// 1.包含头文件
#include "rclcpp/rclcpp.hpp"
#include <std_msgs/msg/float32_multi_array.hpp>
#include <std_msgs/msg/int32.hpp>
#include "std_msgs/msg/float64.hpp"

int state;
float j2_data = 0.0;
float j3_data = 0.0;

// 已知三角形的三边 a、b 和 c，要求夹角 C
void to_cos(const float c_a, const float c_b, const float c_c, float &theta_c)
{
    float numerator = c_a * c_a + c_b * c_b - c_c * c_c;
    float denominator = 2.0f * c_a * c_b;
    float cos_c = numerator / denominator;
    theta_c = acosf(cos_c) * 180.0f / static_cast<float>(M_PI); // 弧度转角度
}

// 3.自定义节点类；
class Endeffector : public rclcpp::Node
{
public:
    Endeffector() : Node("endeffector_node_cpp")
    {
        callback_group_j1 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
        callback_group_j2 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
        callback_group_j3 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
        rclcpp::SubscriptionOptions options_j1;
        options_j1.callback_group = callback_group_j1;
        rclcpp::SubscriptionOptions options_j2;
        options_j2.callback_group = callback_group_j2;
        rclcpp::SubscriptionOptions options_j3;
        options_j3.callback_group = callback_group_j3;
        //
        state_sub = this->create_subscription<std_msgs::msg::Int32>(
            "joint_state", 10,
            std::bind(&Endeffector::state_cb, this, std::placeholders::_1),options_j1);
        // 发送给joint程序
        angle_pub = this->create_publisher<std_msgs::msg::Int32>("joint4_gp", 10);
        // 订阅joint2和joint3的角度
        this->joint2_sub = this->create_subscription<std_msgs::msg::Float64>("/joint2_p", 10, std::bind(&Endeffector::j2_cb, this, std::placeholders::_1), options_j2);
        this->joint3_sub = this->create_subscription<std_msgs::msg::Float64>("/joint3_p", 10, std::bind(&Endeffector::j3_cb, this, std::placeholders::_1), options_j3);
    }

private:
    //
    float p1 = 781.0;
    float p2 = 557.0;
    float p3 = 815.8;
    float p4 = 345.8;
    // 正解所需的余弦定理求解到的长度
    float l2_f, l3_f;
    // 正解余弦转换的角度
    float theta2_F, theta3_F;
    // 运动状态的订阅
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr state_sub;
    // 末端角度的发布
    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr angle_pub;
    //
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr joint2_sub;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr joint3_sub;
    //
    rclcpp::CallbackGroup::SharedPtr callback_group_j2;
    rclcpp::CallbackGroup::SharedPtr callback_group_j3;
    rclcpp::CallbackGroup::SharedPtr callback_group_j1;

    void j2_cb(const std_msgs::msg::Float64::SharedPtr j2_msg)
    {
        j2_data = j2_msg->data;
    }
    void j3_cb(const std_msgs::msg::Float64::SharedPtr j3_msg)
    {
        j3_data = j3_msg->data;
    }
    void state_cb(const std_msgs::msg::Int32::SharedPtr msg)
    {
        state = msg->data;

        l2_f = j2_data + 713.5;
        if (l2_f > p1 + p2)
        {
            l2_f = p1 + p2;
        }
        to_cos(p1, p2, l2_f, theta2_F);
        // 补偿角度
        theta2_F = theta2_F + 9.0 + 5.3;
        //
        l3_f = j3_data + 718.5;
        if (l3_f > p3 + p4)
        {
            l3_f = p3 + p4;
        }
        to_cos(p3, p4, l3_f, theta3_F);
        // 补偿角度
        theta3_F = theta3_F + 11.3 + 27.5;

        auto anglepub = std_msgs::msg::Int32();

        float theta3 = 17.0;
        float theta1, theta2, angle;
        switch (state)
        {
        case 0:
            // 扫上侧
            theta1 = 180.0 - theta3 - theta2_F;
            theta2 = theta3_F - theta1;
            angle = 180.0 - theta2;
            if (angle > 90.0)
            {
                angle = 90.0;
            }
            anglepub.data = angle;

            angle_pub->publish(anglepub);
            std::cout << "case 0 ： 清扫上侧" << std::endl;
            std::cout << "当前角度为：" << angle << std::endl;

            break;
        case 1:
            // 扫两侧
            theta1 = 180.0 - theta3 - theta2_F;
            theta2 = theta3_F - theta1;
            angle = 180 - (90.0 + theta2);
            if (angle > 90.0)
            {
                angle = 90.0;
            }
            anglepub.data = angle;

            angle_pub->publish(anglepub);
            std::cout << "case 1：清扫两侧" << std::endl;
            std::cout << "当前角度为：" << angle << std::endl;

            break;
            // 平放置
        case 2:
            anglepub.data = 0;
            angle_pub->publish(anglepub);
            std::cout << "case 2：平放置" << std::endl;
            std::cout << "当前角度为：" << anglepub.data << std::endl;
            break;

        default:
            break;
        }
    }
};

int main(int argc, char const *argv[])
{
    // 2.初始化ROS2客户端；
    rclcpp::init(argc, argv);
    rclcpp::executors::MultiThreadedExecutor executor;
    auto node = std::make_shared<Endeffector>();
    // 4.调用spain函数，并传入节点对象指针；
    executor.add_node(node);
    executor.spin();
    // 5.资源释放
    rclcpp::shutdown();
    return 0;
}